/***************************************************************************************************************************
* Eight_trajectory.h
*
* Author: SFL
*
* Update Time: 2020.10.12
*
* Introduction:  Eight trajectory generation code
*         1. Generating the Eight trajectory
*         2. Parameter: center, radius, linear_vel, time_total, direction
*         3. Input: time_from_start
*         4. Output: position, velocity, acceleration, jerk, snap
***************************************************************************************************************************/
#ifndef EIGHT_TRAJECTORY_H
#define EIGHT_TRAJECTORY_H

#include <Eigen/Eigen>
#include <math.h>
#include <math_utils.h>
#include <wrzf_pkg/TrajectoryPoint.h>
#include <command_to_mavros.h>

using namespace std;

class Eight_Trajectory
{
     //public表明该数据成员、成员函数是对全部用户开放的。全部用户都能够直接进行调用，在程序的不论什么其他地方訪问。
    public:

        //构造函数
        Eight_Trajectory(void):
            Eight_Trajectory_nh("~")
        {
            // Eight_Trajectory_nh.param<float>("Eight_Trajectory/direction", direction, 1.0);

            Eight_Trajectory_nh.param<float>("Eight_Trajectory/Center_x", eight_origin_[0], 0.0);
            Eight_Trajectory_nh.param<float>("Eight_Trajectory/Center_y", eight_origin_[1], 0.0);
            Eight_Trajectory_nh.param<float>("Eight_Trajectory/Center_z", eight_origin_[2], 1.0);
            Eight_Trajectory_nh.param<float>("Eight_Trajectory/omega", eight_omega_, 0.5);
            Eight_Trajectory_nh.param<float>("Eight_Trajectory/radial", radial, 2.0);
            Eight_Trajectory_nh.param<float>("Eight_Trajectory/time_total", time_total, 10.0);
        }
        
        // Parameter
        Eigen::Vector3f eight_origin_;
        float radial;
        float eight_omega_;
        float time_total;

        //Printf the Eight_Trajectory parameter
        void printf_param();

        //Printf the Eight_Trajectory result
        void printf_result(wrzf_pkg::TrajectoryPoint& Eight_trajectory);

        //Eight_Trajectory Calculation [Input: time_from_start; Output: Eight_trajectory;]
        wrzf_pkg::TrajectoryPoint Eight_trajectory_generation(float time_from_start);

    private:

        ros::NodeHandle Eight_Trajectory_nh;
};


wrzf_pkg::TrajectoryPoint Eight_Trajectory::Eight_trajectory_generation(float time_from_start)
{
    Eigen::Vector3f position;
    Eigen::Vector3f velocity;
    Eigen::Vector3f acceleration;
    
    float angle = eight_omega_* time_from_start;
    const float cos_angle = cos(angle);
    const float sin_angle = sin(angle);
    
    Eigen::Vector3f eight_radial_ ;
    Eigen::Vector3f eight_axis_ ;
    eight_radial_ << radial, 0.0, 0.0;
    // eight_axis_ << 0.0, 0.0, 2.0;
    eight_axis_ << 0.0, 0.0, 1.0;

    position = cos_angle * eight_radial_ + sin_angle * cos_angle * eight_axis_.cross(eight_radial_)
                 + (1 - cos_angle) * eight_axis_.dot(eight_radial_) * eight_axis_ + eight_origin_;

    velocity = eight_omega_ * (-sin_angle * eight_radial_ + (pow(cos_angle, 2) - pow(sin_angle, 2)) * eight_axis_.cross(eight_radial_)
                 + (sin_angle) * eight_axis_.dot(eight_radial_) * eight_axis_);

    acceleration << 0.0, 0.0, 0.0;

    wrzf_pkg::TrajectoryPoint Eight_trajectory;

    Eight_trajectory.header.stamp = ros::Time::now();

    Eight_trajectory.time_from_start = time_from_start;

    Eight_trajectory.Sub_mode = command_to_mavros::XYZ_POS;

    Eight_trajectory.position_ref[0] = position[0];
    Eight_trajectory.position_ref[1] = position[1];
    Eight_trajectory.position_ref[2] = position[2];

    Eight_trajectory.velocity_ref[0] = velocity[0];
    Eight_trajectory.velocity_ref[1] = velocity[1];
    Eight_trajectory.velocity_ref[2] = velocity[2];

    Eight_trajectory.acceleration_ref[0] = 0;
    Eight_trajectory.acceleration_ref[1] = 0;
    Eight_trajectory.acceleration_ref[2] = 0;

    Eight_trajectory.yaw_ref = 0;

    // to be continued...

    return Eight_trajectory;
}



void Eight_Trajectory::printf_result(wrzf_pkg::TrajectoryPoint& Eight_trajectory)
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>> Eight_Trajectory <<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    //固定的浮点显示
    cout.setf(ios::fixed);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout<<setprecision(2);

    cout << "time_from_start : " << Eight_trajectory.time_from_start<< " [s] " <<endl;

    cout << "position  [X Y Z] : " << Eight_trajectory.position_ref[0] << " [m] "<< Eight_trajectory.position_ref[1]<<" [m] "<<Eight_trajectory.position_ref[2]<<" [m] "<<endl;

    cout << "velocity [X Y Z] : " << Eight_trajectory.velocity_ref[0] << " [m/s] "<< Eight_trajectory.velocity_ref[1]<<" [m/s] "<<Eight_trajectory.velocity_ref[2]<<" [m/s] "<<endl;

    cout << "acceleration [X Y Z] : " << Eight_trajectory.acceleration_ref[0] << " [m/s^2] "<< Eight_trajectory.acceleration_ref[1]<<" [m/s^2] "<< Eight_trajectory.acceleration_ref[2]<<" [m/s^2] "<<endl;

    // cout << "jerk [X Y Z] : " << Eight_trajectory.jerk_ref[0] << " [m/s^3] "<< Eight_trajectory.jerk_ref[1]<<" [m/s^3] "<<Eight_trajectory.jerk_ref[2]<<" [m/s^3] "<<endl;

    // cout << "snap [X Y Z] : " << Eight_trajectory.snap_ref[0] << " [m/s^4] "<< Eight_trajectory.snap_ref[1]<<" [m/s^4] "<<Eight_trajectory.snap_ref[2]<<" [m/s^4] "<<endl;

}

// 【打印参数函数】
void Eight_Trajectory::printf_param()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Eight_Trajectory Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    cout <<"Eight Shape:  " <<endl;
    cout <<"eight_origin_ :  "<<endl << eight_origin_ <<endl;
    cout <<"eight_omega_ :  "<< eight_omega_ << endl;
    cout <<"time_total : "<< time_total << endl;
    cout <<"radial : "<< radial << endl;
    //direction = 1 for CCW 逆时针, direction = -1 for CW 顺时针
}



#endif // EIGHT_TRAJECTORY_H
